from numpy import *
from numpy.linalg import *
from ConfigfileReader import ConfigfileReader

##  GHJState contains the functionality behind the GHJPanel.
#   All estimation methods for calculating the GH-Joints are in this class.
#   All methods in this class ending with RegressionMethod are used for the calculation of the GH-Joint using the regression method.
#   All methods in this class ending with MovementMethod are used for the calculations of the GH-Joint using the movement method.
class GHJState:
    ##  The constructor of GHJState.
    #   @param[in]  main    Main wx.App derived instance.
    def __init__(self, main):
        ##  Main wx.App derived instance.
        self.main = main

    ##  Calculates the two GH-Joints using the regression method.
    def CalcRegressionMethod(self):

        connections = self.main.config.ReadGHJointsRegressionConnection()

        # Acquire a dictionary list in which the keys are the bony landmark ID's and the items are their corresponding index in a list.
        # This is essentially mapping to make the code more generic.
        blDict = self.main.GetBonyLandmarkDict(True)
        # Using the GH-Joint data specified in the config file, The GH-Joints calculated using the 5 bony landmarks that are associated with it.
        for ghJointsIndex, blConnections in connections.items():

            sPC = self.main.bonyLandmarks[blDict[blConnections[0]]].relativePosition
            sAC = self.main.bonyLandmarks[blDict[blConnections[1]]].relativePosition
            sAA = self.main.bonyLandmarks[blDict[blConnections[2]]].relativePosition
            sTS = self.main.bonyLandmarks[blDict[blConnections[3]]].relativePosition
            sAI = self.main.bonyLandmarks[blDict[blConnections[4]]].relativePosition


            if sPC.any() and sAC.any() and sAA.any() and sTS.any() and sAI.any():
                if ghJointsIndex == 24:
                    GH = self._GHEstimateRRegressionMethod(sPC, sAC, sAA, sTS, sAI)                    
                else: #ghJointsIndex == 25
                    GH = self._GHEstimateLRegressionMethod(sPC, sAC, sAA, sTS, sAI)
                self.main.bonyLandmarks[blDict[ghJointsIndex]].relativePosition = GH
                #~ if ghJointsIndex == 24:
                    #~ self.LinkGHtoHumerus(GH)


    def LinkGHtoHumerus(self, GH_relativeToScapula):     
        #If you want to link GH to the humerus rather than the scapula, use this function.
        #In the config.ini, link the GH-bonylandmark to the humerus sensor rather than the scapula sensor.
        #fetch Scapula sensor position & rotation
        sensorTransform = self.main.driver.get_sample("fob0_4") #fob0_4
        outputSensor = Sensor("fob0_4")
        outputSensor.rawPosition = sensorTransform[0:3]
        outputSensor.rawRotation = sensorTransform[3:13]
        outputSensor = self.main.driver.CalibrateSingleSensor(outputSensor)
        #~ outputSensor.position/rotation hold our info
                
        #calculate GH-global
        GH_global = self.main.driver.RelativeToGlobal(GH_relativeToScapula, outputSensor.position, outputSensor.rotation)
        
        #fetch Humerus sensor position & rotation
        sensorTransform2 = self.main.driver.get_sample("fob0_5") #fob0_5
        outputSensorHumerus = Sensor("fob0_5")
        outputSensorHumerus.rawPosition = sensorTransform2[0:3]
        outputSensorHumerus.rawRotation = sensorTransform2[3:13]
        outputSensorHumerus = self.main.driver.CalibrateSingleSensor(outputSensorHumerus)
        #~ outputSensorHumerus.position/rotation hold our info
        
        #calculate HumerusGH-relative
        GH_relativeToHumerus = self.main.driver.GetCalcBLVec(GH_global, outputSensorHumerus.position, outputSensorHumerus.rotation)
        
        #set self.main.bonyLandmarks[24].relativePosition
        self.main.bonyLandmarks[24].relativePosition = GH_relativeToHumerus

    ##  Estimates right GH-Joint.
    #   @param[in]  (pc, ac, aa, ts, ai)    The relative position of 5 bony landmarks.
    #   @return[matrix]                     The right GH-Joint estimate.
    def _GHEstimateRRegressionMethod(self, pc, ac, aa, ts, ai):
        return self.__GHEstimateRegressionMethod(pc*10, ac*10, aa*10, ts*10, ai*10)/10

    ##  Estimates left GH-Joint.
    #   @param[in]  (pc, ac, aa, ts, ai)    The relative position of 5 bony landmarks.
    #   @return[matrix]                     The left GH-Joint estimate.
    def _GHEstimateLRegressionMethod(self, pc, ac, aa, ts, ai):
        pcb = pc.copy()
        acb = ac.copy()
        aab = aa.copy()
        tsb = ts.copy()
        aib = ai.copy()
        pcb[1,:] = -pcb[1,:]
        acb[1,:] = -acb[1,:]
        aab[1,:] = -aab[1,:]
        tsb[1,:] = -tsb[1,:]
        aib[1,:] = -aib[1,:]

        GHl = self.__GHEstimateRegressionMethod(pcb*10, acb*10, aab*10, tsb*10, aib*10)/10
        GHl[1,:] = -GHl[1,:]
        return GHl

    ##  Calculates GH from regression equations according to Meskers et al 1997. juli 1996. C. Meskers.
    #   @param[in]  (pc, ac, aa, ts, ai)    The relative position of 5 bony landmarks.
    #   @return[matrix]                     The GH-Joint estimate.
    def __GHEstimateRegressionMethod(self, pc, ac, aa, ts, ai):
        Rsca = self.__AsScap96RegressionMethod(ac, ts, ai)
        Osca = ac

        pc = transpose(Rsca) * (pc - Osca)
        ac = transpose(Rsca) * (ac - Osca)
        aa = transpose(Rsca) * (aa - Osca)
        ts = transpose(Rsca) * (ts - Osca)
        ai = transpose(Rsca) * (ai - Osca)

        lacaa = norm(ac - aa)
        ltspc = norm(ts - pc)
        laiaa = norm(ai - aa)
        lacpc = norm(ac - pc)

        scx = vstack((1, pc[0,0], ai[0,0], laiaa, pc[1,0]))
        scy = vstack((1, lacpc, pc[1,0], lacaa, ai[0,0]))
        scz = vstack((1, pc[1,0], pc[2,0], ltspc))

        thx = matrix("[18.9743 0.2434 0.2341 0.1590 0.0558]")
        thy = matrix("[-3.8791 -0.1002 0.1732 -0.3940 0.1205]")
        thz = matrix("[9.2629 -0.2403 1.0255 0.1720]")

        GHx = thx * scx
        GHy = thy * scy
        GHz = thz * scz
        gh = vstack((GHx, GHy, GHz))
        return (Rsca * gh) + Osca

    ##  Calculation of local coordinate system of the scapula S.
    #   @param[in]  (AC, TS, AI)    The column vectors.
    #   @return[matrix]             The local coordinate system.
    def __AsScap96RegressionMethod(self, AC, TS, AI):
        xs = (AC-TS) / norm(AC-TS)
        zhulp = cross(transpose(xs), transpose((AC-AI)))
        zhulp = zhulp / norm(zhulp)
        ys = cross(zhulp, transpose(xs))
        zs = cross(transpose(xs),ys)

        return hstack((xs, transpose(ys), transpose(zs)))
    
    ##  Method calculating the GH-Joints using the movement method.
    def CalcMovementMethod(self):
        pass